#include "src/astar.h"
#include <conio.h>
#include <iostream>
#include <iterator>

namespace pathfind {

Astar::Astar(std::unordered_map<IntPoint, int> map, int width, int height)
    : graph_(map, width, height), visited_pos_(64), optimized_pos_(64),
      start_(0, 0), end_(0, 0), frontier_(), cost_so_far_(), came_from_(),
      neighbours_() {}

std::vector<IntPoint> Astar::finding(const IntPoint &start,
                                     const IntPoint &end) {
  frontier_ = AstarPriorityQueue();
  cost_so_far_.clear();
  came_from_.clear();
  neighbours_.clear();

  start_ = start;
  end_ = end;

  // 1. end point out of range
  if (graph_.is_position_out_of_range(end_)) {
    end_ = start_;
  }
  // 2. start == end
  if (start_ == end_) {
    optimized_pos_.push_back(start);
    optimized_pos_.push_back(start);
    return optimized_pos_;
  }

  // 3. end point not in walking area, find the neaest point
  // TODO: the 
  if (!graph_.is_position_valid(end_)) {
    end_ = get_neaest_point(end_, 63);
  }

  // 4. normal condition

  frontier_.push(std::make_pair(start_, 0.0f));
  cost_so_far_.insert(std::make_pair(start_, 0.0f));
  came_from_.insert(std::make_pair(start_, start_));

  while (frontier_.size() != 0) {
    const auto cur = frontier_.top().first;
    frontier_.pop();

    if (cur == end_) {
      break;
    }

    neighbours_ = graph_.get_neighbours(cur);
    for (auto neighbour : neighbours_) {
      float new_cost =
          cost_so_far_[cur] + graph_.cost(cur, neighbour.first, end_);
      if (cost_so_far_.find(neighbour.first) == cost_so_far_.end() ||
          new_cost < cost_so_far_[neighbour.first]) {
        cost_so_far_[neighbour.first] = new_cost;

        float priority =
            new_cost + graph_.heuristic(cur, neighbour.first, end_);
        frontier_.push(std::make_pair(neighbour.first, priority));

        came_from_[neighbour.first] = cur;
      }
    }
  }

  reconstruct_path();
  optimizing();

  return optimized_pos_;
}

/*
 * @param p     the target point
 * @param range finding range
 * @return      the neaest point.
 */
IntPoint Astar::get_neaest_point(const IntPoint &p, const int32_t range) const {
  IntPoint p1(0, 0);
  for (int32_t x = 1; x <= range; x++) {
    for (int32_t i = -x; i <= x; i++) { // radius
      if (std::abs(i) == x) {           // border <left, right>
        for (int32_t j = -x; j <= x; j++) {
          p1.set(p.x + i, p.y + j);
          if (graph_.is_position_valid(p1)) {
            return p1;
          }
        }
      } else { // <top, bottom>
        p1.set(p.x + i, p.y + x);
        if (graph_.is_position_valid(p1)) {
          return p1;
        }

        p1.set(p.x + i, p.y - x);
        if (graph_.is_position_valid(p1)) {
          return p1;
        }
      }
    }
  }

  return p1;
}

void Astar::reconstruct_path() {
  auto cur = end_;
  visited_pos_.clear();

  visited_pos_.push_back(cur);

  while (cur != start_) {
    cur = came_from_[cur];
    visited_pos_.push_back(cur);
  }

  visited_pos_.push_back(start_);
  std::reverse(visited_pos_.begin(), visited_pos_.end());
}

void Astar::optimizing() {
  if (visited_pos_.size() == 0) {
    return;
  } else if (visited_pos_.size() <= 3) {
    std::copy(visited_pos_.begin(), visited_pos_.end(),
              std::back_inserter(optimized_pos_));
    return;
  }

  optimized_pos_.clear();

  std::vector<IntPoint> poly_nodes;
  DIRECTION prev_direction = DIRECTION::STILL;
  DIRECTION cur_direction = DIRECTION::STILL;

  IntPoint curpos;
  for (size_t i = 0; i < visited_pos_.size(); i++) {
    curpos = visited_pos_[i];
    if (i + 1 == visited_pos_.size()) {
      poly_nodes.push_back(curpos);
      break;
    } else {
      cur_direction = graph_.get_direction(curpos, visited_pos_[i + 1]);
      if (cur_direction != prev_direction) {
        poly_nodes.push_back(curpos);
      }

      prev_direction = cur_direction;
    }
  }

  std::copy(poly_nodes.begin(), poly_nodes.end(),
            std::back_inserter(optimized_pos_));
}

bool Astar::ComparerCost::operator()(const PairPositionCost &ppc1,
                                     const PairPositionCost &ppc2) {
  return ppc1.second > ppc2.second;
}
}
